/*
WICHTIG TODO: Ein ordentliches sleep implementieren

*/

#include <floppy.h>
#include <printThings.h>
#include <ports.h>
#include <mytime.h>

#define SLEEPTIME 50
#define TIM 1

#define floppy_dmalen 512
static const char floppy_dmabuf[floppy_dmalen] __attribute__((aligned(0x8000)));

unsigned char floppyIntFired = 0;
int base = 0x3F0;

void waitFloppyIrq() {
    while (floppyIntFired == 0) {}
    floppyIntFired = 0;
    return;
}

void handleFloppyInterrupt() {
    floppyIntFired = 1;
}

int checkInterrupt() {
    sendCmdToTheFloppy(GET_INTERRUPT_STATUS);
    unsigned char err = readDataFromTheFloppy();
    unsigned char c = readDataFromTheFloppy();


    switch (err) {
    case 0x0: {
        printf("OK\n");
        break;
    }
    case 0x1: {
        printf("der Befehl wurde gestartet, aber nicht richtig beendet\n");
        break;
    }
    case 0x2: {
        printf("invalid Command\n");
        break;
    }
    case 0x3: {
        printf("Controller war nicht bereit (Polling)\n");
        break;
    }
    }

    return c;
}


void sendCmdToTheFloppy(unsigned char cmd) {
    int i;
    volatile unsigned msr;

    for (i = 0; i < 5000; i++) {
        msr = inb(base + FLOPPY_MAIN_STATUS_REGISTER);
        if ((msr & 0xC0) == 0x80){
            outb(base + FLOPPY_FIFO_DATA_REGISTER, cmd);
            inb(base + FLOPPY_MAIN_STATUS_REGISTER);
            return;
        }
        sleepMilliSeconds(5);
    }

    /*der befehl konnte nicht abgesetzt werden*/
    setColor(RED);
    printf("FLOPPY ERROR: konnte Befehl nicht absetzen!\n");
    setStandard();
}

unsigned char readDataFromTheFloppy() {
    int i;
    volatile unsigned msr;

    for (i = 0; i < 5000; i++) {
        msr = inb(base + FLOPPY_MAIN_STATUS_REGISTER);
        if ((msr & 0xC0) == 0xC0) { // from 0xD0 zu 0xC0
            /*nun koennen wir das kommando absetzen*/
            return inb(base + FLOPPY_FIFO_DATA_REGISTER);
        }

        sleepMilliSeconds(5);
    }

    setColor(RED);
    printf("FLOPPY ERROR: konnte nicht vom Floppy lesen\n");
    setStandard();
    return 0x0;
}


void motorOn() {
    //printf("MotorON\n");
    // schreibe 0001 1100 in das DOR register
    /*    0         0        0          1           1            1            0              0
    	MotorD    MotorC    MotorB    MotorA    IRQ und DMA    ENABLED    DRIVENUMBER    DRIVENUMBER
    	 BIT7      BIT6      BIT5      BIT4        BIT3         BIT2          BIT1           BIT0
    	wir brauchen den motor f端r das erste floppy also den ersten motor DMA und IRQ haben wir und
    	das bit2 schaltet das ganze ein
    */
    outb(base + FLOPPY_DIGITAL_OUTPUT_REGISTER, 0x1C);
    /*bissi warten eigentlich 300ms aber einen so genauen timer hab ich zz noch nicht*/
    sleepMilliSeconds(SLEEPTIME);
}

void motorOff() {
    //printf("MotorOFF\n");
    //schreibe 00001100 in das DOR register
    outb(base + FLOPPY_DIGITAL_OUTPUT_REGISTER, 0x0C);
    sleepMilliSeconds(SLEEPTIME);
}

void calibrateFloppy() {
    motorOn();

    sendCmdToTheFloppy(RECALIBRATE_COMMAND);
    sendCmdToTheFloppy(0);/*dieser parameter gibt das laufwerk an*/

    waitFloppyIrq();
    int cyl = checkInterrupt();

    motorOff();

    if (cyl == 0) {
        setColor(GREEN);
        printf("[OK]\n");
        setStandard();
    }
}

void programmDMA(int mode_r) {
    asm volatile("sti");

    union {
        unsigned char b[4];
        unsigned long l;
    } a, c;

    a.l = (unsigned) &floppy_dmabuf;
    c.l = (unsigned) floppy_dmalen - 1;

    unsigned char mode;
    switch (mode_r) {
    case MODE_READ:
        mode = 0x46;
        break;
    case MODE_WRITE:
        mode = 0x4a;
        break;
    }

    outb(0x0a, 0x06);   // channel 2 (fuer das floppy) maskieren
    outb(0x0c, 0xff);   // flipFlop resetten (irgendeinen wert an 0x0c schreiben)
    outb(0x04, a.b[0]); // die addresse byte eins zuweisen
    outb(0x04, a.b[1]); // die addresse byte zwei zuweisen
    outb(0x81, a.b[2]); // external page register
    outb(0x0c, 0xff);   // wieder das flipflop resetten weil wir wieder in den speicher schreiben
    outb(0x05, c.b[0]); // wohin wollen wir zaehlen(byte1)
    outb(0x05, c.b[1]); // wohin wollen wir zaehlen(byte2)
    outb(0x0b, mode);   // sodale jetzt setzen wir den modus den wir oben erarbeitet haben
    outb(0x0a, 0x02);   // channel 2 wieder demaskieren

    asm volatile("sti");
}

int seekSektor(unsigned char cylinder, unsigned char head) {
    /* aus lowlevel.brainsware.org:
      *	Seek command = 0xf
     * First parameter byte = (head number << 2) | drive number (the drive number must match the currently selected drive!)
     * Second parameter byte = requested cylinder number
     * No result bytes.
     * The interrupt may take up to 3 seconds to arrive, so use a long timeout.
      */
    int i = 0;

    for (i = 0; i < 10; i++) {
        sendCmdToTheFloppy(SEEK_COMMAND);
        sendCmdToTheFloppy(head << 2);
        sendCmdToTheFloppy(cylinder);

        waitFloppyIrq();
        int c = checkInterrupt();

        sleepMilliSeconds(15);

        while (inb(0x3F4) & 0x0F) {
           printf("Laufwerk ist noch beschaeftigt\n");
           sleepMilliSeconds(1);
        }

        if (c == cylinder)
            return 1;
    }

    return -1;
}

unsigned char* readSectorLBA(int lba) {

    int cylinder = lba / (2 * 18);
    int temp = lba % (2 * 18);
    int head = temp / 18;
    int sektor = temp % 18 + 1;

    //printf("lba: %d\n", lba);
    //printf("head: %d, cylinder: %d, track: %d\n", head, cylinder, sektor);

    return readSector(head, cylinder, sektor);
}

unsigned char* readSector(unsigned char head, unsigned char cylinder, unsigned char track) {
    unsigned char readCommand = READ_COMMAND | 0xC0;

    motorOn();

    //printf("seek\n");
    if (seekSektor(cylinder, head) == -1) {
        setColor(RED);
        printf("ERROR: Seek ist fehlgeschlagen\n");
        setStandard();
    }

    programmDMA(MODE_READ);

    //printf("sendRead Cmd Cylinder(%d)...", cylinder);
   
    sendCmdToTheFloppy(readCommand);  // sagen wir dem floppy wir wollen lesen
    sendCmdToTheFloppy(head << 2);    // welchen head wollen wir?
    sendCmdToTheFloppy(cylinder);
    sendCmdToTheFloppy(head);
    sendCmdToTheFloppy(track);
    sendCmdToTheFloppy(2);    			// wieviele bytes pro sektor brauchen wir
    sendCmdToTheFloppy(18);   // damit wir auch nicht mit den tracks drueberkommen
    sendCmdToTheFloppy(0x1b);        // GAP3 konstante fuer unsere floppys ka was die soll
    sendCmdToTheFloppy(0xff); 
    //printf("ok\n");

    //printf("Wait Irq...");
    floppyIntFired = 0;
    waitFloppyIrq();
    //printf("Fired\n");
   
    unsigned char st0, st1, st2, rcy, rhe, rse, bps;
    st0 = readDataFromTheFloppy();
    st1 = readDataFromTheFloppy();
    st2 = readDataFromTheFloppy();
    rcy = readDataFromTheFloppy();
    rhe = readDataFromTheFloppy();
    rse = readDataFromTheFloppy();
    bps = readDataFromTheFloppy();

    int error = 0;

    setColor(RED);
    if (st0 & 0xC0) {
        printf("error: status(%d) = ", st0 >> 6);
        
        switch (st0 >> 6) {
            case 0x0: {
                printf("OK\n");
                break;
            }
            case 0x1: {
                printf("der Befehl wurde gestartet, aber nicht richtig beendet\n");
                break;
            }
            case 0x2: {
                printf("invalid Command\n");
                break;
            }
            case 0x3: {
                printf("Controller war nicht bereit (Polling)\n");
                break;
            }
         }

        error = 1;
    }
    if (st1 & 0x80) {
        printf("error: end of cylinder\n");
        error = 1;
    }
    if (st0 & 0x08) {
        printf("error: drive not ready\n");
        error = 1;
    }
    if (st1 & 0x20) {
        printf("error:CRC error\n");
        error = 1;
    }
    if (st1 & 0x10) {
        printf("error:controller timeout\n");
        error = 1;
    }
    if (st1 & 0x04) {
        printf("error:no data found\n");
        error = 1;
    }
    if ((st1|st2) & 0x01) {
        printf("error:no address mark found\n");
        error = 1;
    }
    if (st2 & 0x40) {
        printf("error:deleted address mark\n");
        error = 1;
    }
    if (st2 & 0x20) {
        printf("error:CRC error in data\n");
        error = 1;
    }
    if (st2 & 0x10) {
        printf("error:wrong cylinder\n");
        error = 1;
    }
    if (st2 & 0x04) {
        printf("error:uPD765 sector not found\n");
        error = 1;
    }
    if (st2 & 0x02) {
        printf("error:bad cylinder\n");
        error = 1;
    }
    if (bps != 0x2) {
        printf("error:wanted 512B/sector, got %d", (1<<(bps+7)));
        error = 1;
    }
    if (st1 & 0x02) {
        printf("error:not writable\n");
        error = 2;
    }
    setStandard();

    motorOff();

    return (unsigned char *)&floppy_dmabuf;
}

void flpydskDriveData(unsigned long stepr, unsigned long loadt, unsigned long unloadt, int dma )
{
	unsigned long data = 0;
	sendCmdToTheFloppy (SPECIFY_COMMAND); // send command
	data = ((stepr & 0xf) << 4) | (unloadt & 0xf);
	sendCmdToTheFloppy(data);
	data = (loadt) << 1 | (dma==FALSE) ? 0 : 1;
	sendCmdToTheFloppy(data);
}

void initFloppy() {
    outb(0x3F2, 0);
    motorOff();
    outb(0x3F4, 0x00);
    outb(0x3F4, 0x0C);
    
    waitFloppyIrq();
    int i;
    for(i=0; i<4; ++i)
        checkInterrupt();

    sendCmdToTheFloppy(SPECIFY_COMMAND);
    sendCmdToTheFloppy(0xDF);
    sendCmdToTheFloppy(0x02);

    calibrateFloppy();
}
